import numpy as np

def get_nd_sincos_pos_embed(embed_dim, grid_size, cls_token=False):
    dims = len(grid_size)
    if dims == 3:
        return get_3d_sincos_pos_embed(embed_dim, grid_size, cls_token)
    elif dims == 2:
        return get_2d_sincos_pos_embed(embed_dim, grid_size, cls_token)
    elif dims == 1:
        return get_1d_sincos_pos_embed(embed_dim, grid_size, cls_token)


def get_1d_sincos_pos_embed(embed_dim, grid_size, cls_token=False):
    grid = np.arange(grid_size[0], dtype=np.float32)
    grid = grid.reshape([1, 1] + grid_size)
    pos_embed = get_1d_sincos_pos_embed_from_grid(embed_dim, grid)
    if cls_token:
        pos_embed = np.concatenate([np.zeros([1, embed_dim]), pos_embed], axis=0)
    return pos_embed

def get_2d_sincos_pos_embed(embed_dim, grid_size, cls_token=False):
    grid_h = np.arange(grid_size[0], dtype=np.float32)
    grid_w = np.arange(grid_size[1], dtype=np.float32)
    grid = np.meshgrid(grid_w, grid_h)  # here w goes first
    grid = np.stack(grid, axis=0)

    grid = grid.reshape([2, 1] + grid_size)
    pos_embed = get_2d_sincos_pos_embed_from_grid(embed_dim, grid)
    if cls_token:
        pos_embed = np.concatenate([np.zeros([1, embed_dim]), pos_embed], axis=0)
    return pos_embed

def get_3d_sincos_pos_embed(embed_dim, grid_size, cls_token=False):
    grid_h = np.arange(grid_size[0], dtype=np.float32)
    grid_w = np.arange(grid_size[1], dtype=np.float32)
    grid_d = np.arange(grid_size[2], dtype=np.float32)
    
    grid = np.meshgrid(grid_w, grid_h, grid_d)  # here w goes first
    grid = np.stack(grid, axis=0)

    grid = grid.reshape([3, 1] + grid_size)
    pos_embed = get_3d_sincos_pos_embed_from_grid(embed_dim, grid)
    if cls_token:
        pos_embed = np.concatenate([np.zeros([1, embed_dim]), pos_embed], axis=0)
    return pos_embed

def get_3d_sincos_pos_embed_from_grid(embed_dim, grid):
    d1 = (embed_dim // 3)
    d2 = (embed_dim // 3)
    d3 = embed_dim - d1 - d2

    # use half of dimensions to encode grid_h
    emb_h = get_1d_sincos_pos_embed_from_grid(d1, grid[0])
    emb_w = get_1d_sincos_pos_embed_from_grid(d2, grid[1])
    emb_d = get_1d_sincos_pos_embed_from_grid(d3, grid[2])
    emb = np.concatenate([emb_h, emb_w, emb_d], axis=1)
    return emb

def get_2d_sincos_pos_embed_from_grid(embed_dim, grid):
    d1 = (embed_dim // 2)
    d2 = embed_dim - d1

    # use half of dimensions to encode grid_h
    emb_h = get_1d_sincos_pos_embed_from_grid(d1, grid[0])  # (H*W, D/2)
    emb_w = get_1d_sincos_pos_embed_from_grid(d2, grid[1])  # (H*W, D/2)

    emb = np.concatenate([emb_h, emb_w], axis=1) # (H*W, D)
    return emb

def get_1d_sincos_pos_embed_from_grid(embed_dim, pos):
    """
    embed_dim: output dimension for each position
    pos: a list of positions to be encoded: size (M,)
    out: (M, D)
    """
    # assert embed_dim % 2 == 0
    omega = np.arange(embed_dim // 2, dtype=np.float64)
    omega /= embed_dim / 2.
    omega = 1. / 10000**omega  # (D/2,)

    pos = pos.reshape(-1)  # (M,)
    out = np.einsum('m,d->md', pos, omega)  # (M, D/2), outer product

    emb_sin = np.sin(out) # (M, D/2)
    emb_cos = np.cos(out) # (M, D/2)

    pad = np.zeros((pos.shape[0], embed_dim % 2))
    emb = np.concatenate([emb_sin, emb_cos, pad], axis=1)  # (M, D)
    return emb
